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摘 要 : 位 姿 图 优化 (pose graph optimization，PGO) 是 计算 机 视觉 领域 中 广泛 应 用 的 高 维 非 凸 优化 算法 ， 很 难 直 接 求 
解 ， 主 要 依赖 于 和 迭代 技术 ， 对 初始 值 的 质量 要 求 较 高 ， 在 实践 中 很 难得 到 保证 。 针 对 位 姿 图 优化 问题 进行 了 研究 ， 
提出 了 基于 特征 分 解 的 位 姿 图 简单 封闭 解 算法 ， 该 算法 首先 对 PGO 问题 的 最 大 似 然 估 计 进 行 半 定 松弛 ， 然 后 将 其 
转换 为 特征 分 解 问题 ， 并 利 FI ACIE IHR 性 设计 了 改进 的 模型 降 阶 方法 进行 求解 ， 进 一 步 提 高 了 算法 的 计算 速度 。 
算法 具有 可 伸缩 性 、 计 算 成 本 低 和 精度 高 等 优点 。 最 后 ， 在 模拟 和 真实 的 位 姿 图 数据 集 上 进行 实验 评估 ， 结 果 表 明 
在 不 影响 精度 的 ' 清 况 下 ， 该 算法 可 以 快速 地 进行 位 姿 图 优化 。 
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Fast pose optimization algorithm based on eigen decomposition 


Li Yujie?, Wei Guoliang"', Cai Jiea, Wang Miaomiaos 
(a. College of Science, University of Shanghai for Science & Technology, b. Business School, University of Shanghai for 
Science & Technology, Shanghai 200093, China) 


Abstract: Pose graph optimization (PGO) is a high dimensional non-convex optimization algorithm widely used in the field 
of computer vision. It is hard to solve directly and mainly relies on iterative techniques. It requires high quality of initial value 
which is difficult to be guaranteed in practice. This paper studied PGO problem and proposed a simple closed solution 
algorithm for pose graph based on eigen decomposition. This algorithm first developed the semidefinite relaxation of 
maximum-likelihood estimation (MLE) for PGO problems. Then it transformed MLE into eigen decomposition problem, and 
designed an improved model reduction method to solve the problem by using the sparsity of data, which further improves the 
computational speed of the algorithm. The algorithm has the advantages of scalability, low computational cost and high 
precision. Finally, experimental evaluation on simulated and real-world pose-graph datasets shows that the proposed algorithm 
can optimize the pose graph quickly without compromising accuracy. 

Key words: pose graph optimization; maximum-likelihood estimation; eigen decomposition; model reduction 
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0 引言 表示 机 器 人 的 绝对 位 姿 ， 边 表示 相对 测量 值 ， 然后 对 图 进行 
优化 ,得 到 更 精确 的 结果 。.PGO 的 目标 问题 通常 是 高 维 的 7， 
C SLAM(Simultaneous Localization and Mapping) 是 在 机 器 涉及 很 多 变量 和 约束 ,所 以 对 优化 算法 的 计算 效率 要 求 很 高 。 
= 人 人 缺乏 环境 先 验 信 息 的 情况 下 ， 通 过 传感器 的 测量 值 在 对 自 此 外 由 于 目标 问题 的 非 凸 性 ， 在 求解 时 容易 陷入 局 部 极 小 值 
身 运 动 进行 估计 的 同时 建立 环境 模型 由 ， 这 对 在 未 知 场景 中 使 估计 完全 无 用 ， 所 以 在 保证 计算 效率 的 基础 上 ， 避 免 陷 入 
进行 自主 导航 和 探索 至 关 重 要 。 近 年 来 ，SLAM 技术 广泛 应 局 部 极 小 值 是 PGO 问题 的 重 中 之 重 
于 日 常生 活 中 ,例如 自动 驾驶 外 ,搜索 救援 申 ， 精密 农业 中 前 PGO 问题 的 求解 算法 主要 划分 为 三 类 四, 第 一 类 算 
等 ， 成 为 研究 热点 。SLAM 系统 分 为 前 端 和 后 端 ， 如 图 1 所 法 是 利用 问题 的 全 局 结构 进行 迭代 求解 。Lu 和 Milios PIH 1X 
示 。 前 端 视觉 里 程 计 (visula odometry, VO)， 是 根据 相 邻 图 像 是 出 用 位 姿 图 表示 SLAM 问题 ， 并 进行 迭代 非 线 性 优化 。 
古 计 相机 之 间 的 相对 运动 ， 但 这 个 过 程 存在 累积 误差 ， 需 要 Olson 等 人 09 提 出 用 随机 梯度 下 降 方法 求解 PGO 问题 , 该 方 
后 端 对 前 端 输出 的 结果 进行 优化 ， 得 到 最 优 的 位 姿 估 计 和 全 ” 法 具有 良好 的 稳定 性 和 可 扩展 性 , 即使 给 定 较 差 的 初始 估计 ， 
局 一 致 加 的 地 图 ， 提 高 SLAM 系统 的 精度 。 也 可 以 快速 优化 机 器 人 的 位 姿 。Grisetti 等 人 0 改进 了 Olson 
pue. = 等 人 的 工作 ， 使 用 树 状 结构 定义 和 更 新 局 部 区 域 ， 显 著 提 高 
NN = aii O| x 了 随机 梯度 下 降 方法 的 收敛 性 。Kaess & AU2- He Hi TE 
> 平滑 算法 ， 该 类 算法 可 以 更 好 地 处 理 非 线性 测量 模型 ， 通 过 
— P) me 增 量 重新 排序 提高 了 优化 效率 ， 可 以 实现 大 规模 PGO 问题 
~ ~ ~ ~ 的 更 新 。Rosen 等 人 05 研 究 了 PGO 问题 的 最 小 二 乘 结构 ， 指 
图 1 SLAM 系统 框架 出 了 降低 其 非 线性 和 非 凸 性 的 可 能 性 。Kiimmerle 等 人 09 利 
Fig. 1 Architecture of SLAM system 用 图 结构 的 稀疏 性 ， 使 用 高 斯 -牛顿 方法 进行 求解 。 然 而 ， 上 
PGO 是 目前 机 器 人 领域 中 应 用 最 广泛 的 SLAM 后 端 优 ” 述 所 有 的 非 线性 优化 技术 都 是 局 部 搜索 方法 ， 并 不 能 保证 解 
RISI, i SLAM 问题 用 图 进行 直观 表示 ， 其 中 图 的 顶点 的 最 优 性 。 
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对 角 阵 ， 对 角 线 上 的 元 素 为 各 个 顶点 的 度 ， 即 aL. S 


第 二 类 算法 则 在 寻找 良好 的 初始 值 ， 以 提高 收敛 速度 并 
降低 收敛 到 局 部 极 小 值 的 风险 ,Carlone 等 人 07 提 出 了 LAGO 
算法 ， 为 2D PGO 问题 提供 了 精确 的 初始 估计 ， 但 是 LAGO 
法 只 能 应 对 中 低 噪 声 数 据 集 。 之 后 在 文献 [18] 中 对 几 种 先 
进 的 初始 化 方法 进行 了 回顾 和 比较 ， 结 果 表 明 ， 使 用 弱 方 法 
作为 初始 化 器 ,GTSAM 作为 求解 器 是 迭代 求解 的 最 佳 组 合 。 
Nasiri 等 人 09 提 出 了 RS 算法 求解 PGO 问题 ， 该 算法 在 高 噪 
声 下 也 能 得 到 最 优 解 的 精确 近似 ， 具 有 较 好 的 鲁 棒 性 。 
第 三 类 算法 是 对 PGO 问题 进行 松弛 ， 以 克服 目标 问题 
的 非 凸 性 。Carlone 等 人 对 原 问题 进行 半 定 松弛 , 利用 拉 格 朗 
日 对 偶 理论 验证 了 2DP0 和 3DP!U SLAM 中 解 的 最 优 性 ; 
证 明了 当 对 偶 间 隅 为 零 时 , 可 以 检索 出 原 问 题 的 全 局 最 优 解 。 
Rosen 等 人 P 利 用 PGO 问题 的 低 秩 、 几 何 结构 , 将 其 简化 为 
低 维 黎 曼 流 形 上 的 等 价 优化 问题 ， 并 设计 了 一 种 截断 信任 域 
方法 进行 求解 , 比 使 用 内 点 法 求解 快 几 个 数量 级 。Eriksson 等 
人 P3 利 用 图 论 对 旋转 平均 问题 进行 了 理论 分 析 ， 求 解 出 了 噪 
声 水 平 的 误差 界 ， 以 确保 解 的 全 局 最 优 性 。 然 而 ， 因 为 这 些 
解 是 通过 松弛 约束 得 到 的 ,所 以 通常 不 在 原 问题 的 可 行 域内 ， 
需要 重新 投影 。 
综 上 ， 目 前 PGO 的 相关 算法 可 以 收敛 到 全 局 最 优 近似 
解 , 但 是 计算 速度 较 慢 , 计算 成 本 过 高 。 为 了 加 快 求解 效率 ， 
Singer24 提 出 通过 特征 分 解 进行 角 同 步 的 计算 ， 但 是 只 能 应 
用 于 2D 问题 ， 不 能 扩展 到 更 常用 的 3D 问题 。 之 后 在 文献 
[25] 中 , 将 特征 分 解 方法 扩展 到 了 3D 问题 , 但 是 只 计算 了 位 
姿 的 旋转 部 分 ， 忽 略 了 平移 部 分 的 优化 。Arrigoni 等 人 29 提 
出 用 特征 分 解 方法 解决 运动 同步 问题 ， 同 时 对 旋转 和 平移 进 
了 优化 ( 即 在 线性 群 SEO 上 进行 优化 )， 但 是 增 大 了 优化 矩阵 
维度 ， 随 着 问题 规模 的 增加 ， 求 解 速度 会 受到 很 大 影响 ; 
并 且 为 了 最 终 得 到 满足 SEO) 形式 的 解 ， 牺 牲 了 估计 的 精度 。 
本 文 在 上 述 问题 的 基础 上 提出 了 基于 特征 分 解 的 快速 位 
姿 图 优化 算法 ， 即 EDPO 算法 。 该 算法 分 为 两 个 阶段 进行 优 
化 : 第 一 阶段 使 用 改进 的 特征 分 解 算法 优化 旋转 矩阵 ， 第 二 
个 阶段 使 用 最 小 二 乘法 求解 平移 部 分 。 该 算法 具有 可 伸缩 性 、 
计算 成 本 低 和 精度 高 等 优点 。 本 文 的 主要 贡献 如 下 : 
a) 提 出 了 一 种 新 颖 的 PGO 算法 ， 将 原 PGO 问题 划分 为 
两 个 子 问题 。 首 先 对 旋转 矩阵 的 行列 式 约束 进行 松弛 ， 推 导 
出 了 基于 特征 分 解 的 封闭 解 ， 再 将 解 投影 到 正 交 群 上 以 满足 
旋转 矩阵 的 性 质 ;然后 使 用 最 小 二 乘法 优化 位 姿 的 平移 部 分 。 
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图 2 ERR 
Fig.2 Complete graph 
加 权 图 是 由 图 G=(V,E) 和 定义 在 其 边 上 的 权重 函数 w 组 


的 三 联 体 G=(V,E,w)。 在 一 些 应 用 中 , 给 出 了 反映 测量 值 可 
性 的 非 负 权 重 w， 将 其 存储 在 图 的 邻接 矩阵 4=[w] 中 ， 因 
， 加 权 图 的 度 和 矩阵 D 定义 为 B=2 w 。 
1.2 位 姿 图 优化 


位 姿 图 是 图 优化 的 一 种 


体形 式 ， 其 中 图 的 顶点 表示 机 


器 人 的 绝对 位 姿 ， 边 表示 相对 测量 值 ， 本 文 将 其 用 于 解决 机 
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1.2.1 测量 值 的 生成 模型 


b) 提 出 了 一 个 简洁 的 矩阵 公式 , 将 原 PGO 问题 的 公式 改 
写 为 可 以 进行 特征 分 解 的 形式 , 与 对 应 的 图 结构 的 Laplacian 
抢 阵 建立 了 联系 ， 以 便 进 行 特征 分 解 ， 并 且 该 矩阵 公式 可 以 
处 理 数据 缺失 的 情况 ， 计 算 更 精确 。 
c) 为 了 提高 特征 分 解 模型 的 计算 性 能 ， 提 出 了 一 种 改进 
的 模型 降 阶 方法 ， 高 效 地 解决 了 推导 出 的 特征 分 解 模 型 的 高 
维 问 题 。 
d) 在 模拟 和 真实 的 PGO 数据 集 上 的 对 比 实验 表明 , 在 不 
影响 精度 的 情况 下 ，EDPO 算法 显著 提高 了 计算 效率 。 


1 ”位 姿 图 优化 理论 基础 
1.1 图 的 理论 基础 


N 


布 ; 
Mises-Fisher 分 布 。 
1.2.2 最 大 似 然 估 计 


图 结构 可 以 用 G=(V,E) 表 示 ， 其 中 V 是 非 空 集 合 称 为 节 
点 集 ，E 是 V 中 元 素 构成 的 无 序 二 元 组 的 集合 ， 称 为 边 集 ， 
本 文中 车 点数 和 边 数 分 别 用 n 和 m 表示 。 若 每 对 不 同 的 顶点 
之 间 都 恰 有 一 条 边 相连 ， 则 该 图 为 完全 图 (如 图 2). 
4 Aeg 为 G 的 邻接 矩阵 ， 其 元 素 为 

inue 0) 
0,(i, j) E 

很 显然 ，4 为 对 称 和 矩阵 。 令 DeR™ Jy G 的 度 矩 阵 ， 则 D 


A- 


OHENEE Dl 


大 


人 的 轨迹 优化 问题 。 


PGO 计算 在 给 定 m 对 相对 测量 值 的 条 件 下 , n 个 机 器 人 


Eus 的 最 大 似 然 估 计 。 


在 三 维 问题 中 ， 机 器 人 的 绝对 


姿 和 相对 测量 值 都 是 特殊 欧式 群 SEO) 中 的 元 素 


*Y ,旋转 矩阵 需要 同时 满足 正 交 约束 


行列 式 约束 。 本 文 使 用 符号 =(R,f) RI x, =R 4) 32S 


对 位 姿 和 相对 测量 值 。 


本 节 提 出 一 个 修改 后 的 PGO 公式 ， 其 中 将 使 用 弦 距 离 
来 量化 旋转 误差， 从 测量 值 的 生成 模型 开始 ， 然 后 推导 出 相 


的 最 大 似 然 估 计 。 


假设 以 下 是 相对 测量 值 ,如 的 生成 模型 


量 
1j; =R] (t; 51) 15,5; ~ gaussian(0,, 01) 


R= RRIR,,R, ~ vonMises(L,, c.) 


rH gaussian( u, Q) 表示 均 值 为 
vonMises(S ,k) 表示 均 值 为 


Q) 


4 和 协 方差 矩阵 为 OQ 的 高 斯 分 
S， 参 数 为 k 的 各 向 同性 的 von 


求解 位 姿 的 最 大 似 然 估计 (MLBE) 等 价 于 最 小 化 负 对 数 似 然 : 


fu - min, -logL( |x) -log Lt; |x). G) 
E UO (E J)EE 
— — 
-log L(R; |x) = ELS -RR,||. + const. (4) 
-log L(Ę |x) = vj |t; ^t; - Rit I + const. (5) 


将 式 (4) 和 (5) 代 入 到 式 (3) 


中 ， 必 表示 矩阵 的 Frobenius 范 数 。 


中 ， 得 到 最 大 似 然 估计 : 


o M S 
f= mim Youln-n-ALP*TIR-RA[L © 


(rem 4 
(Reso(3)) JEE 


MLE 问题 是 高 维 、 非 凸 


此 ， 将 原 优化 问题 划分 为 下 


、 好 5p 
的 ， 假设 JR -BR 是 式 


Res0(3) 2 


F 移 的 求解 变 成 一 个 最 小 二 乘 问题 1， 


个 子 问题 : 
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L(R, A) =1rCRT7COM7AMDJR)+1rCACRTR 
Kp, Ac-bikdiag(A,...., ^,) e IR?"?" 为 拉 格 朗 
格 朗 日 定理 


2(MTM)R+2RA=0 

HI (MTM)R--RA , 4r (i1,2,3)73 M*M 

向 量 ， 且 元 是 对 应 的 特 和 有 

约束 R'R-nL, 。 也 就 是 说 , 任意 三 个 特征 向 量 

BH H EKI 

EI ARRE E DU S 8] pe] ERAS) c 7] 

问题 转换 为 求解 矩阵 的 特征 向 量 问题 
最 终 求 得 了 问题 式 (18) 的 封闭 解 ， 但 这 

式 下 求 得 的 解 ， 可 能 不 完全 满足 旋转 矩阵 的 

更 精确 的 旋转 估计 值 , 用 下 表示 命题 1 得 到 


通过 奇异 值 分 解 =U L V 找到 与 其 最 接近 
令 启 =Uvr"， 再 进一步 强制 执行 det&)=1， 得 
的 解 。 


2.2 改进 的 模型 降 阶 方法 


—-nlj) 


P, 将 工 对 了 的 偏 导 数 设置 为 零 ， 


正 值 ， 则 Rn. 0.51 同时 满足 式 (21) 及 


数 的 一 个 稳定 点 。 如 果 nG-12) 是 MTM 的 最 小 特征 
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Q0) 
HRT, WHH 
可 得 到 稳定 性 


Q1) 
的 任意 三 个 特征 


都 可 以 组 成 拉 格 


` 值 , 所 以 原 优 化 


是 原 问 题 松 弛 形 
性 质 ， 为 了 得 到 
的 包含 特征 向 量 


的 3nx3 的 矩阵 。 其 中 玫 表 示 第 ;个 位 次 旋转 的 估计 值 ， 需 要 


的 旋转 和 矩阵， 并 
到 满足 旋转 约束 


在 旋转 优化 模型 建立 后 ， 需 要 进行 特 行 


F 值 和 特征 


向 量 的 


求解 。 因 为 在 实际 应 用 中 需要 求解 的 矩阵 是 
使 用 传统 算法 会 大 量 填充 零 元 素 ， 严 重 消 耗 
寻求 有 效 的 求解 方法 。 


录用 定稿 李 雨 洁 
min 4R - RA; 0) 
min; lt n -RG (8) 
在 接 下 来 的 部 分 中 ， 将 先 求解 问题 式 (7)， 然 后 用 得 到 的 
最 优 旋转 解 来 求解 问题 式 (8)。 
2 ”本 文 方法 
2.1 旋转 优化 模型 
为 了 求解 旋转 问题 式 (7)， 下 面 证 明 通 过 特征 分 解 可 以 找 
到 一 个 全 局 最 优 旋转 解 R 。 为 方便 进行 推导 计算 ,在 后 面 的 
Erb EUR BERE, 将 所 有 位 次 的 旋转 矩阵 堆 营 在 Re Rw 中 
R=[R?,..., RT ,R e SOQ) (9) 
首先 考虑 所 有 相对 测量 值 都 可 用 ( 即 完 全 图 )， 并 且 相 对 
测量 值 不 含 噪声 的 情况 ， 将 旋转 测量 值 坊 堆肥 在 块 矩 阵 
Re Rn "m, Bp 
I, Ro R, 
n MR o) 
Ra Ra e d 
AET RRR, WA 
R= RR" 
m ap 
KJ RR=n,, npJf$j 
RR=nR (12) 
这 意味 着 在 没有 噪声 的 情况 下 ， 的 列 是 由 玉 的 与 特征 
值 n 对 应 的 特征 向 量 构 成 的 ， 因 为 的 秩 为 3， 所 以 其 余 特 
征 值 均 为 零 ，n 就 是 R 的 最 大 特征 值 。 式 (12) 等 价 于 式 (13) 
(nls—R)R=0 (13) 
其 次 考虑 相对 运动 测量 被 噪声 破坏 的 情况 ， 式 (12) 对 应 
的 优化 问题 为 
min S [RR nf; (14) 
此 处 放松 了 旋转 矩阵 的 行列 式 约 束 ， 即 在 “02G) 中 进行 


优化 ， OQ S(IRERH?:RR-L), 
然而 在 实际 应 用 中 并 非 所 有 相对 测量 值 都 可 用 ， 所 以 下 

机 考虑 数据 缺失 的 情况 。 缺 失 的 相对 运动 对 应 天 中 的 零 块 ， 

(4@1)* 人 表示 可 用 的 相对 运动 信息 , 其 中 ，@ 表示 Kronecker 


积 ，。 表 示 Hadamard 积 。 邻 接 和 矩阵 4 在 与 1; 进行 Kronecker 
乘积 后 被 扩 维 ， 以 匹配 R ERASMI. 在 这 种 情况 下 ， 式 (12) 可 
以 推广 为 
((A8GL)eR)R - (DG I,)R (15) 
等 价 于 
((DG1I,)-(AG1,)o R)R-0 (16) 
观察 到 D8 对 应 于 (D81)oR ， 因 为 的 对 角 线 上 的 元 
素 是 单位 阵 ， 并 且 D@ 是 块 对 角 阵 。 所 以 式 (16) 可 以 再 进 
步 改写 为 
((L@1)°o RR=0 (17) 
当 品 声 存在 时 ， 上 式 不 完全 成 立 ， 对 应 的 优化 问题 为 


2 
min |MR|, 


(18) 


其 中 ， M-(L81).R, 
命题 1 问题 式 (18) 存 在 一 个 封闭 解 ，1 
征 值 相 关 的 三 个 特征 向 量 给 出 。 


M'M 的 最 小 特 


模型 降 阶 方法 的 初始 化 阶段 进行 了 改进 。 
Arnoldi 模型 降 阶 方法 ， 


高 维 且 稀 玻 的 
内 存 ， 所 以 需要 


目前 常用 的 方法 是 Arnoldi 模型 降 阶 
方法 R99， 该 方法 用 一 个 较 小 系统 近似 原始 系统 ， 稳 定性 强 ， 
计算 速度 快 , 适用 于 大 规模 的 特征 求解 问题 。 


本 文 在 Arnoldi 


首先 要 建立 矩阵 M7M 的 Krylov 


子 空间 的 一 组 标准 正 交 基 Qv) , 3x ZH AE 493 t E E SES 
代 计 算 

w-(M'M)v, (22) 

得 到 w ， 然 后 再 将 w 与 之 前 的 疝 量 进行 正 交 ， 得 到 。 


系统 ， 所 以 本 文 


然而 ， 仅 通过 一 次 正 交 化 可 能 得 不 到 稳定 的 
在 正 交 化 之 后 又 加 入 重 正 交 化 步 又， 这 样 可 
空间 的 基 具 有 较 好 的 正 交 性 。 算 法 1 总 结 了 
方法 。 
算法 1 改进 的 模型 降 阶 方法 

输入 : 对 称 和 矩阵 M7M RUE I8 EE vi 

输出 : MTM 的 特征 值 与 特征 向 量 4,9(i=1,2,… 

步骤 1: forj=1:r-1 

w=(M"M)v; 


for i-l;j // 正 交 化 
hj 2v Ww;w = wo— hyvi; 

end 

fori-lj ”// 重 正 交 化 


S$ =v Ww; 


h; =h; +s;w=w-sv;; 


end 


stop 
else vj =W/hjaj 
end 
Via 2IV;.v;al 
end 
H, =[h,,] 
(M' MV, 2 VH, + h, v, 4e! 


以 保证 得 到 的 子 
改进 的 模型 降 阶 


r); 


/* 得 到 rr 2b Arnoldi 分 解 , 其 


证 明 问题 式 (18) 等 价 于 
minfr(R" (M" M)R) 
s.t.R; € O(3) (19) 


对 应 的 拉 格 朗 日 函 


2 
ERE RE I 的 最 后 一 行 */ 
3 


: 计算 矩阵 H, 的 特征 


Ls 


n -1,2,...,r); 


值 和 对 应 的 特征 


向 量 ， 分 别 为 
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步骤 4: SERE MTM. 的 特征 值 和 特征 向 量 分 别 为 3.1 模拟 噪声 实验 

4=h,0 =V mi =1,2..r); 为 了 探究 测量 噪声 (包括 旋转 和 平移 )、 问 题 规 模 大 小 对 

步骤 5: 返回 AQ T L2, n); EDPO 算法 性 能 的 影响 ， 所 以 进行 了 一 组 模拟 实验 。 首 先 向 

2.3 ”基于 特征 分 解 的 封闭 解 算法 原始 cube 数据 子 集 的 相对 测量 值 添加 噪声 , 该 数据 集 模拟 了 

通过 2.1 节 的 旋转 优化 模型 找到 了 最 优 解 R* 后 , 问题 式 。 机 器 人 沿 直 线 穿 过 规则 立方 体 的 轨迹 。 测 量 值 与 =%, 态 根据 

(8) 可 以 写 为 式 (2) 进 行 采样 ， 其 中 ， 旋 转 噪声 的 标准 差 oRE[0*, 107], ^F 
minzilr, -6 - R5. gay CA EEEE T [0 us - 

bu 然后 分 别 在 不 同 噪声 水 平 (包括 旋转 和 平移 )、 不 同 规模 


tat 


与 2.1 节 类 似 ， 使 用 块 矩 阵 进 行 推 导 ， 将 平移 向 量 “大 小 的 cube 数据 集 上 , 对 比 了 EDPO 算法 和 G-N 算法 、EIG 


o1.) ERE re m 中 SEI EIE EECÉHL UAZ. FCH FERRO UOS ER 
rebar Q4) 。 数 ， 所 以 数值 越 小 说 明 结果 越 精确 。 所 有 的 统计 数据 都 是 经 

因此 ， 式 (23) 可 以 重 写 为 过 100 次 随机 实验 取 平均 值得 到 的 ， 模 拟 实验 的 结果 如 下 。 
ER qs, 其 中 ,图 3 的 GD) 分 别 描述 旋转 噪声 (平移 噪声 固定 为 oT-0.1， 

2 位 姿 数量 固定 为 5 对 三 种 算法 的 目标 函数 值 和 运行 时 间 的 
Kuh, penes 是 包含 权重 的 邻接 矩阵 , 在 与 及 进行 Kronecker — 影响 ， 从 图 中 可 以 看 出 ，EDPO 算法 的 速度 比 G-N 法 和 EIG 


乘积 后 被 扩 维 ,以 匹配 : 的 结构 ;CsRs" 是 包含 相对 平移 测量 。 算法 要 快 得 多 ,目标 函数 值 与 G-N 法 基本 持平 。 这 组 模拟 实 


HERE, CHUA Kk [TAE RIS, (eG DeE). WA, RE 验 的 结果 表明 ，EDPO 算法 在 合理 的 操作 条 件 下 可 以 得 到 
个 线性 最 小 二 乘 问题 ， 封 闭 解 为 SLAM 问题 的 全 局 最 优 近 似 解 。 
t*=(B"B)'B"C (26) 
于 878 是 对 称 的 , 可 以 使 用 Cholesky 分 解 进行 预 处 理 ， 2300 
这 样 可 以 加 快 计算 逆 和 矩阵 的 速度 。 结 合 前 面 的 旋转 优化 模型 ， 2! 
得 到 了 完整 的 EDPO 算法 ， 总 结 在 了 算法 2 中 。 
算法 2 EDPO 算法 js aso. 
输入 : 图 的 节点 V 、 边 E 、 相 对 测量 值 (Rj) 以 及 权重 ki ti i 
输出 : 位 姿 估 计 R,iG=1,2,.…,n) T: 2200 } 
步 又 1: 将 原 PGO 问题 划分 为 式 (7)(8) 两 个 子 问题 ; 
2100 上 
4382: RETKI min IR -RRi 
2000 : : i i 
1.for e -(üjeE 0 2 4 6 8 10 
AG, j) = kj DG.) = DD + ky; 旋转 噪声 
R(block(i, j)) = Ry; R(block(i,i)) = L; (a) 旋转 噪声 对 目标 函数 的 影响 
end 
L-D-A; 0.5 
2. M -(LG1)9R sal SUR 
3. 用 算法 1 求解 M7M 的 最 小 特征 值 和 对 应 的 三 个 特征 向 量 ， El 
记 为 7(i=1,2,3) Œ 0.31 
4.for i=1:n js 
F, 2r(3*i-2:3*i,3y; "o2. ] 
[U,, 2. V,] - SVDCE,); 
R, =UV"; 0.1F 1 
end j | 
| zs 0 2 4 6 8 10 
步骤 3: 求解 子 问题 min c; tr, 7r, -RT| — 
1.fo e -(j)eE (b) 旋转 噪声 对 运行 时 间 的 影响 
B(k.i) - n; B(k, j) = n; 图 3 不 同 水 平 的 旋转 噪声 对 算法 性 能 的 影响 
C*&k -2:3*k) =R, tj; Fig.3 Influence of different levels of rotating noise on 
end algorithm performance 
2. t -(B'By'B'C; 同时 ， 还 记录 了 三 种 算法 的 误差 对 比分 析 结 果 ， 包 括 平 
t 2t (8*i-2:3*1); 均 误 差 、 中 值 误 差 和 均 方 根 误差 ， 误差 越 小 ， 表 示 算 法 越 稳 
步骤 4: 返回 Ri,fi(i=1,2,.…,n) ; 定 鲁 棒 性 越 好 。 实验 结果 分 别 记录 在 图 4 的 (a)(b)(c) 中 。 可 以 


MEN 看 到 , 旋转 噪声 的 标准 差 oRE[0", 10], EDPO 算法 的 误差 
3 ”实验 结果 与 分 析 值 小 于 EIG 算法 ， 与 G-N 法 基本 持平 ， 这 表明 EDPO 算法 
在 本 节 中 ， 分 别 在 模拟 和 真实 的 3D PGO 数据 集 上 进行 ”在 一 定 的 旋转 噪声 水 平 下 可 以 恢复 机 器 人 的 位 姿 轨迹 。 

实验 , 将 提出 的 EDPO 算法 与 高 斯 一 牛顿 法 ( 记 为 G-N)、EIG 5 的 (a)(b) 分 别 描 述 平移 噪声 (旋转 噪声 固定 为 oR=5°, 
算法 29 进 行 对 比 ， 评 估 EDPO 算法 的 性 能 。 以 下 所 有 实验 都 ”位 姿 数量 固定 为 5 对 三 种 算法 的 目标 函数 和 运行 时 间 的 影 
是 在 Dell OptiPlex 7040 微型 台式 机 电脑 上 进行 的 ， 实验 运行 响 ， 从 图 中 可 以 看 出 ，EDPO 的 速度 比 G-N 法 和 EIG 法 要 
环境 配置 为 Intel& Core i5-6500 CPU @ 3.20GHz, 4GB 内 存 ， 快 , 目标 函数 值 与 G-N 法 基本 持平 , 所 以 总 体 性 能 要 优 于 G- 
运行 Windows 10 系统 。 N 法 和 EIG 法 。 
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1.1 13r 
"GN 12 pag 
1 —-OUR asap 
—-—EIG 14 —-EIG 
3k 0.9 W 1 
m m 
EY im 
8- 0.8 5g. 09 
0.8 
0.7 
0.7 
0.6 0.6 0.7 
0 2 4 6 8 10 0 2 6 8 10 0 2 4 6 8 10 
旋转 噪声 旋转 噪声 旋转 噪声 
(a) 平均 误差 (b) 中 值 误差 (c) 均 方 根 误差 
图 4 优化 结果 的 误差 比较 
Fig.4 Error comparison of optimization results 
2800 0.5 
2600 上 04r 
3 2400 上 pr 0.3 
iE uz 
i i 
T 2200: 1K 0.2 
2000 上 0.1 
, ， a í 0 : : i 
18005 d 04 058 0.8 1 0 0.2 0.4 0.6 0.8 1 
平移 噪声 平移 噪声 
(a) 平 移 噪声 对 目标 函数 的 影响 (b) 平 移 噪声 对 运行 时 间 的 影响 
图 5 不 同 水 平 的 平移 噪声 对 算法 性 能 的 影响 
Fig.5 Influence of different levels of translation noise on algorithm performance 
图 6 的 (a)(b) 分 别 描述 位 姿 数量 的 增加 对 三 种 算法 性 能 3.2 算法 性 能 对 比 实验 


的 影响 (旋转 噪声 固定 为 GaR=5$， 平 移 噪声 固定 为 oT-0.1), 
从 图 中 可 以 看 出 ， 问 题 规 模 的 增加 对 EDPO 运行 时 间 的 影响 
要 小 于 G-N 法 和 EIG 算法 ， 并 且 


目标 函数 值 与 G-N 法 基本 持 


Fig. 6 


平 ， 这 验证 了 EDPO 算法 具有 可 伸缩 性 ， 适 用 于 大 规模 问题 。 


à x10* 
24 
FERT 
T 12 
0.6 
0 
0 2000 4000 6000 8000 10000 
位 姿 数量 
(a) 位 姿 数 量 对 目标 函数 的 影响 
30 r 
—*— G-N 
—9—- OUR 
24r |——EIG 
iz 18Tr 
is 
"2- 
6l | 
> pen dm a 
0 2000 4000 6000 8000 10000 
位 姿 数量 


(b) 位 姿 数量 对 运行 时 间 的 影响 
图 6 位 姿 数量 对 算法 性 能 的 影响 


The influence ofpose number on algorithm performance 


选择 了 6 个 公开 的 3D SLAM 数据 集 作为 本 文 的 实验 数 


1 3D 激光 扫 


量 (m) 整 理 在 了 表 1 中 。 


运 

走 的 轨迹 进行 了 模拟 。garage 是 收集 自 
集 ; cubicle、rim 是 RIM 中 心 
" 
x 


昌 集 模拟 了 机 器 人 在 


据 集 , 其 中 sphere, torus 和 cube 数据 集 是 模拟 数据 集 , cubicle, 
rim 和 garage 是 真实 数据 集 。sphere itj 
球面 上 运动 的 轨迹 ; torus 数据 集 模拟 了 机 器 人 在 环 面 表面 上 
运动 时 的 轨迹 ; cube 数据 集 是 对 机 器 人 在 三 维 网 格 世 界 上 行 


Vertigo 的 


" 
改 集 的 两 个 真实 数据 集 ， 通 过 


仪 获取 的 点 云 进行 ICP 获取 
十 于 每 个 数据 集 ， 都 将 相应 的 


了 相对 位 姿 约束 。 


入 姿 数量 (n) 和 相对 测量 值 的 数 


del 数据 集 信息 
Tab. 1 Datasets information 

数据 集 n m 
Sphere 2200 8647 
Torus 5000 9047 
Cube 8000 22236 
Rim 10195 29743 
Cubicle 5750 16869 
Garage 1661 6275 


在 本 部 分 实验 中 ， 将 在 以 上 3D SLAM 数据 集 上 评估 


EDPO 的 性 能 ， 


数据 集 上 的 运行 结果 ， 包 括 数据 集 的 初始 值 、 求 解 的 
数值 、 算 法 的 运行 时 间 以 及 EDPO $13 


这 一 组 数据 集 更 好 地 表示 了 实际 应 用 中 的 问 


题 分 布 。 表 2 记录 了 EDPO 算法 、G-N 法 和 EIG 算法 在 每 个 
标 函 

k 求 得 的 数据 集 的 最 小 

的 轨迹 与 地 面 真实 轨 


特征 值 。 表 3 记录 了 三 种 算法 优化 得 到 


迹 之 间 的 误差 结果 对 比 ， 
误差 以 及 标准 差 。 


包括 平均 误差 、 


通过 表 2 


可 以 看 到 在 每 个 数据 集 


P 值 误差 、 均 方 根 


Ph， 得 到 的 结果 与 3.1 


节 的 模拟 实验 结果 相似 。 在 优化 结果 方面 ，EDPO 在 torus、 
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cube、cubicle、garage 四 个 数据 集 上 得 到 的 解 与 G-N 法 持平 ， 


优化 结果 (机 器 人 轨迹 ) 如 图 7 所 示 ; 在 sphere 和 rim 两 个 数 


据 集 上 求解 的 结果 比 G-N 法 差 
以 看 到 ， 在 本 节 比 较 的 六 个 数 


差 要 低 于 EIG 算法 , 与 G-N 法 基本 持 习 
方法 中 ，EIG 算法 的 优化 结果 要 差 于 EDPO 算法 和 G-N 法 ， 
Ay EIG 算法 将 旋转 和 平移 同时 优化 ， 


解 得 到 四 个 特征 向 量 ， 


， 但 是 差异 较 小 。 
居 集 中，EDPO AEn 
FE。 综 上 ,在 三 种 优化 


通过 表 3 可 
LEVA 


只 进行 单一 的 特征 分 
然后 再 投影 到 SEQ) E, 为 了 满足 SEQ) 


法 
求 
位 


速度 要 远 远 快 
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男 一 方面 ， 


通过 表 2 的 相关 数据 可 以 看 出 ，EDPO 的 求 
于 G-N 法 和 EIG 算法 ， 这 是 因为 EDPO 算 
执行 直接 的 全 局 优化 ， 用 直接 法 对 机 器 人 的 位 姿 进行 优化 


c 


解 ， 计 算 复 杂 度 更 低 ; 而 G-N 法 是 迭代 搜索 技术 ， 需 要 对 
万 化 直至 收敛 ， 所 以 运行 时 间 在 很 大 程度 


姿 不 断 进行 迭代 
取决 于 和 迭代 次 数 ，EIG 算法 将 位 姿 的 旋转 部 分 和 平移 部 分 


同 


杂 


时 进行 优化 ， 扩 大 了 位 姿 表示 的 矩阵 维 数 ， 增 大 了 计算 复 
度 ， 因 此 要 慢 于 EDPO 算法 。 所 以 EDPO 算法 在 求解 速度 


的 形式 ， 导 致 得 到 的 平移 估计 不 是 最 优 的 。EDPO 算法 先 对 上 更 具 优 势 , 在 速度 至 关 重 要 的 低 噪声 水 平 应 用 中 , EDPO $ 
旋转 矩阵 进行 优化 ， 然 后 用 得 到 的 最 优 解 求解 平移 部 分 ， 保 ”法 的 结果 与 G-N 法 结果 之 间 的 区 别 可 以 忽略 不 计 ， 因 此 ， 该 
证 平移 估计 的 准确 性 。 算法 可 以 快速 地 求解 得 到 一 个 很 好 的 解 。 
表 2 3DSLAM 数据 集 的 实验 结果 
Tab.2 Experimental results of a 3D SLAM datasets 
数据 集 初始 值 
入 f time(s) f time(s) f time(s) 
Sphere 1.291x10* 4.53x10? 2.972x10? 0.16 2.657x10? 3.04 3.393x10? 0.43 
Torus 1.996x109* 1.53x10? 2.422x10* 1.27 2.422x10* 10.66 2.475x10* 4.54 
Cube 7.327x107 7.51x10? 8.432x10* 3.12 8.432x10* 599.05 8.733x10* 8.16 
Rim 4.532x107 1.28x10? 8.983x10* 1.98 8.602x10* 43.76 4.147x10? 5.81 
Cubicle 4.994x10* 9.43x10' 4.643x10* 2.63 4.643x10* 338.01 7.227x104 7.2 
Garage 8.36x10? 4.25x107 1.288x10? 0.28 1.288x10? 1.53 1.422x10? 0.57 
表 3 3D SLAM 数据 集 的 误差 结果 对 比 
Tab.3 Comparison of error results in 3D SLAM datasets 
EDPO G-N EIG 
Tt 平均 误差 中 值 误 差 均 方 根 误差 标准 差 平均 误差 中 值 误 差 均 方 根 误差 标准 差 平均 误差 中 值 误差 均 方 根 误差 标准 差 
Sphere 0.554 0.549 0.578 0.171 0.546 0.538 0.571 0.167 0.672 0.659 0.701 0.215 
Torus 0.802 0.741 0.908 0.426 0.802 0.742 0.908 0.426 0.986 0.989 1.064 0.527 
Cube 0.834 0.866 0.899 0.334 0.833 0.864 0.896 0.333 1.114 0.876 1.195 0.486 
Rim 0.671 0.679 0.697 0.190 0.648 0.657 0.675 0.186 0.847 0.895 0.902 0.231 
Cubicle — 0.723 0.702 0.787 0.316 0.721 0.667 0.787 0.309 0.943 0.937 1.047 0.428 
Garage 0.388 0.352 0.419 0.159 0.388 0.351 0.419 0.159 0.552 0.541 0.624 0.199 
(a) Torus 数据 集 (b) Cubicle 数据 集 (c) Cube 数据 集 (d) Garage 数据 集 
图 7 EDPO 优化 结果 
Fig.7 EDPO optimization results 
这 些 结果 进一步 证 明了 EDPO 提供 了 一 种 有 效 的 方法 ， 集 上 进行 实验 ， 结 果 表 明 ，EDPO 能 够 在 合理 的 操作 条 件 下 
可 以 恢复 真实 操作 条 件 下 的 SLAM 的 全 局 最 优 近似 解 , 而 且 ”快速 恢复 SLAM 问题 的 全 局 最 优 近 似 解 。 在 以 后 的 工作 中 ， 
在 计算 速度 上 具有 明显 的 优势 。 计 算 速 度 的 提高 可 以 提升 ”希望 能 将 本 文 算 法 整合 到 一 个 完整 的 SLAM 系统 中 , 以 进 


PGO 技术 的 优 


化 性 能 ， 对 于 基于 图 的 SLAM， 随 着 位 姿 图 维 


数 的 增加 ， 计 算 复 杂 度 增 大 ，EDPO 算法 可 以 提供 一 种 平衡 


精度 和 时 间 的 有 效 方法 。 
4 ARE 


位 姿 ， 然而， 所 涉及 的 优化 策 
初始 化 才能 得 到 


fr PGO 问题 中 ， 最 大 似 然 估计 能 够 以 较 高 的 精度 求解 
过 于 繁琐 ， 


IE 


全 局 最 优 近似 解 。 本 文 提 出 了 基于 特征 分 解 


规模 的 SLAM 问题 。 


的 封闭 解 算法 ， 该 算法 可 以 快速 进行 优化 求 
该 算法 由 两 部 分 组 成 ， 


进行 优化 求解 ， 得 到 旋转 最 优 解 后 再 对 平移 求解 ， 并 日 


需要 良好 的 


解 ， 并 适用 于 大 
先 针对 旋转 部 分 
针对 


该 模型 特点 引入 了 模型 降 阶 方法 ， 在 不 影 


向 精度 的 情况 下 ， 


极 大 地 提高 了 计算 效率 。 最后， 


在 模 把 


和 真实 的 SLAM 数据 
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